{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "介绍如何使用 CommonRoad Scenario Designer 的地图转换 API 来转换场景文件"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 准备\n",
    "\n",
    "import os\n",
    "from lxml import etree\n",
    "\n",
    "from commonroad.scenario.scenario import Tag\n",
    "from commonroad.common.file_writer import CommonRoadFileWriter, OverwriteExistingFile\n",
    "from commonroad.planning.planning_problem import PlanningProblemSet\n",
    "from commonroad.common.file_reader import CommonRoadFileReader\n",
    "\n",
    "ROOT_DIR = os.path.join(os.getcwd(), '..')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "downloading map\n"
     ]
    },
    {
     "ename": "KeyboardInterrupt",
     "evalue": "",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mKeyboardInterrupt\u001b[0m                         Traceback (most recent call last)",
      "\u001b[1;32m/home/string/Programs/ad_benchmark_learn/commonroad/commonroad_scenario_designer_tutorial.ipynb 单元格 3\u001b[0m line \u001b[0;36m6\n\u001b[1;32m      <a href='vscode-notebook-cell:/home/string/Programs/ad_benchmark_learn/commonroad/commonroad_scenario_designer_tutorial.ipynb#W2sZmlsZQ%3D%3D?line=2'>3</a>\u001b[0m \u001b[39mfrom\u001b[39;00m \u001b[39mcrdesigner\u001b[39;00m\u001b[39m.\u001b[39;00m\u001b[39mmap_conversion\u001b[39;00m\u001b[39m.\u001b[39;00m\u001b[39mosm2cr\u001b[39;00m \u001b[39mimport\u001b[39;00m config\n\u001b[1;32m      <a href='vscode-notebook-cell:/home/string/Programs/ad_benchmark_learn/commonroad/commonroad_scenario_designer_tutorial.ipynb#W2sZmlsZQ%3D%3D?line=3'>4</a>\u001b[0m \u001b[39mfrom\u001b[39;00m \u001b[39mcrdesigner\u001b[39;00m\u001b[39m.\u001b[39;00m\u001b[39mmap_conversion\u001b[39;00m\u001b[39m.\u001b[39;00m\u001b[39mmap_conversion_interface\u001b[39;00m \u001b[39mimport\u001b[39;00m opendrive_to_commonroad\n\u001b[0;32m----> <a href='vscode-notebook-cell:/home/string/Programs/ad_benchmark_learn/commonroad/commonroad_scenario_designer_tutorial.ipynb#W2sZmlsZQ%3D%3D?line=5'>6</a>\u001b[0m download_around_map(config\u001b[39m.\u001b[39;49mBENCHMARK_ID \u001b[39m+\u001b[39;49m \u001b[39m'\u001b[39;49m\u001b[39mdownload.osm\u001b[39;49m\u001b[39m'\u001b[39;49m, \u001b[39m31.2708\u001b[39;49m, \u001b[39m121.168\u001b[39;49m, \u001b[39m2000\u001b[39;49m)\n\u001b[1;32m      <a href='vscode-notebook-cell:/home/string/Programs/ad_benchmark_learn/commonroad/commonroad_scenario_designer_tutorial.ipynb#W2sZmlsZQ%3D%3D?line=6'>7</a>\u001b[0m \u001b[39m# 使用 API 的方式下载地图失败，已经手工下载到 scenario/commonroad/map.osm\u001b[39;00m\n",
      "File \u001b[0;32m~/Programs/ad_benchmark_learn/.venv/lib/python3.10/site-packages/crdesigner/map_conversion/osm2cr/converter_modules/osm_operations/downloader.py:106\u001b[0m, in \u001b[0;36mdownload_around_map\u001b[0;34m(filename, lat, lon, radius)\u001b[0m\n\u001b[1;32m     92\u001b[0m \u001b[39m\u001b[39m\u001b[39m\"\"\"\u001b[39;00m\n\u001b[1;32m     93\u001b[0m \u001b[39mdownloads map around center point and saves it to file\u001b[39;00m\n\u001b[1;32m     94\u001b[0m \n\u001b[0;32m   (...)\u001b[0m\n\u001b[1;32m    103\u001b[0m \u001b[39m:return: None\u001b[39;00m\n\u001b[1;32m    104\u001b[0m \u001b[39m\"\"\"\u001b[39;00m\n\u001b[1;32m    105\u001b[0m lon1, lat1, lon2, lat2 \u001b[39m=\u001b[39m get_frame(lon, lat, radius)\n\u001b[0;32m--> 106\u001b[0m download_map(config\u001b[39m.\u001b[39;49mSAVE_PATH \u001b[39m+\u001b[39;49m filename, lon1, lat1, lon2, lat2)\n",
      "File \u001b[0;32m~/Programs/ad_benchmark_learn/.venv/lib/python3.10/site-packages/crdesigner/map_conversion/osm2cr/converter_modules/osm_operations/downloader.py:60\u001b[0m, in \u001b[0;36mdownload_map\u001b[0;34m(filename, lon1, lat1, lon2, lat2)\u001b[0m\n\u001b[1;32m     58\u001b[0m query \u001b[39m=\u001b[39m \u001b[39m'\u001b[39m\u001b[39mhttps://api.openstreetmap.org/api/0.6/map?bbox=\u001b[39m\u001b[39m{}\u001b[39;00m\u001b[39m,\u001b[39m\u001b[39m{}\u001b[39;00m\u001b[39m,\u001b[39m\u001b[39m{}\u001b[39;00m\u001b[39m,\u001b[39m\u001b[39m{}\u001b[39;00m\u001b[39m'\u001b[39m\u001b[39m.\u001b[39mformat(lon1, lat1, lon2, lat2)\n\u001b[1;32m     59\u001b[0m \u001b[39mprint\u001b[39m(\u001b[39m\"\u001b[39m\u001b[39mdownloading map\u001b[39m\u001b[39m\"\u001b[39m)\n\u001b[0;32m---> 60\u001b[0m data \u001b[39m=\u001b[39m urlopen(query)\u001b[39m.\u001b[39mread()\n\u001b[1;32m     61\u001b[0m \u001b[39mwith\u001b[39;00m \u001b[39mopen\u001b[39m(filename, \u001b[39m\"\u001b[39m\u001b[39mwb\u001b[39m\u001b[39m\"\u001b[39m) \u001b[39mas\u001b[39;00m file:\n\u001b[1;32m     62\u001b[0m     file\u001b[39m.\u001b[39mwrite(data)\n",
      "File \u001b[0;32m/usr/lib/python3.10/urllib/request.py:216\u001b[0m, in \u001b[0;36murlopen\u001b[0;34m(url, data, timeout, cafile, capath, cadefault, context)\u001b[0m\n\u001b[1;32m    214\u001b[0m \u001b[39melse\u001b[39;00m:\n\u001b[1;32m    215\u001b[0m     opener \u001b[39m=\u001b[39m _opener\n\u001b[0;32m--> 216\u001b[0m \u001b[39mreturn\u001b[39;00m opener\u001b[39m.\u001b[39;49mopen(url, data, timeout)\n",
      "File \u001b[0;32m/usr/lib/python3.10/urllib/request.py:519\u001b[0m, in \u001b[0;36mOpenerDirector.open\u001b[0;34m(self, fullurl, data, timeout)\u001b[0m\n\u001b[1;32m    516\u001b[0m     req \u001b[39m=\u001b[39m meth(req)\n\u001b[1;32m    518\u001b[0m sys\u001b[39m.\u001b[39maudit(\u001b[39m'\u001b[39m\u001b[39murllib.Request\u001b[39m\u001b[39m'\u001b[39m, req\u001b[39m.\u001b[39mfull_url, req\u001b[39m.\u001b[39mdata, req\u001b[39m.\u001b[39mheaders, req\u001b[39m.\u001b[39mget_method())\n\u001b[0;32m--> 519\u001b[0m response \u001b[39m=\u001b[39m \u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49m_open(req, data)\n\u001b[1;32m    521\u001b[0m \u001b[39m# post-process response\u001b[39;00m\n\u001b[1;32m    522\u001b[0m meth_name \u001b[39m=\u001b[39m protocol\u001b[39m+\u001b[39m\u001b[39m\"\u001b[39m\u001b[39m_response\u001b[39m\u001b[39m\"\u001b[39m\n",
      "File \u001b[0;32m/usr/lib/python3.10/urllib/request.py:536\u001b[0m, in \u001b[0;36mOpenerDirector._open\u001b[0;34m(self, req, data)\u001b[0m\n\u001b[1;32m    533\u001b[0m     \u001b[39mreturn\u001b[39;00m result\n\u001b[1;32m    535\u001b[0m protocol \u001b[39m=\u001b[39m req\u001b[39m.\u001b[39mtype\n\u001b[0;32m--> 536\u001b[0m result \u001b[39m=\u001b[39m \u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49m_call_chain(\u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49mhandle_open, protocol, protocol \u001b[39m+\u001b[39;49m\n\u001b[1;32m    537\u001b[0m                           \u001b[39m'\u001b[39;49m\u001b[39m_open\u001b[39;49m\u001b[39m'\u001b[39;49m, req)\n\u001b[1;32m    538\u001b[0m \u001b[39mif\u001b[39;00m result:\n\u001b[1;32m    539\u001b[0m     \u001b[39mreturn\u001b[39;00m result\n",
      "File \u001b[0;32m/usr/lib/python3.10/urllib/request.py:496\u001b[0m, in \u001b[0;36mOpenerDirector._call_chain\u001b[0;34m(self, chain, kind, meth_name, *args)\u001b[0m\n\u001b[1;32m    494\u001b[0m \u001b[39mfor\u001b[39;00m handler \u001b[39min\u001b[39;00m handlers:\n\u001b[1;32m    495\u001b[0m     func \u001b[39m=\u001b[39m \u001b[39mgetattr\u001b[39m(handler, meth_name)\n\u001b[0;32m--> 496\u001b[0m     result \u001b[39m=\u001b[39m func(\u001b[39m*\u001b[39;49margs)\n\u001b[1;32m    497\u001b[0m     \u001b[39mif\u001b[39;00m result \u001b[39mis\u001b[39;00m \u001b[39mnot\u001b[39;00m \u001b[39mNone\u001b[39;00m:\n\u001b[1;32m    498\u001b[0m         \u001b[39mreturn\u001b[39;00m result\n",
      "File \u001b[0;32m/usr/lib/python3.10/urllib/request.py:1391\u001b[0m, in \u001b[0;36mHTTPSHandler.https_open\u001b[0;34m(self, req)\u001b[0m\n\u001b[1;32m   1390\u001b[0m \u001b[39mdef\u001b[39;00m \u001b[39mhttps_open\u001b[39m(\u001b[39mself\u001b[39m, req):\n\u001b[0;32m-> 1391\u001b[0m     \u001b[39mreturn\u001b[39;00m \u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49mdo_open(http\u001b[39m.\u001b[39;49mclient\u001b[39m.\u001b[39;49mHTTPSConnection, req,\n\u001b[1;32m   1392\u001b[0m         context\u001b[39m=\u001b[39;49m\u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49m_context, check_hostname\u001b[39m=\u001b[39;49m\u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49m_check_hostname)\n",
      "File \u001b[0;32m/usr/lib/python3.10/urllib/request.py:1348\u001b[0m, in \u001b[0;36mAbstractHTTPHandler.do_open\u001b[0;34m(self, http_class, req, **http_conn_args)\u001b[0m\n\u001b[1;32m   1346\u001b[0m \u001b[39mtry\u001b[39;00m:\n\u001b[1;32m   1347\u001b[0m     \u001b[39mtry\u001b[39;00m:\n\u001b[0;32m-> 1348\u001b[0m         h\u001b[39m.\u001b[39;49mrequest(req\u001b[39m.\u001b[39;49mget_method(), req\u001b[39m.\u001b[39;49mselector, req\u001b[39m.\u001b[39;49mdata, headers,\n\u001b[1;32m   1349\u001b[0m                   encode_chunked\u001b[39m=\u001b[39;49mreq\u001b[39m.\u001b[39;49mhas_header(\u001b[39m'\u001b[39;49m\u001b[39mTransfer-encoding\u001b[39;49m\u001b[39m'\u001b[39;49m))\n\u001b[1;32m   1350\u001b[0m     \u001b[39mexcept\u001b[39;00m \u001b[39mOSError\u001b[39;00m \u001b[39mas\u001b[39;00m err: \u001b[39m# timeout error\u001b[39;00m\n\u001b[1;32m   1351\u001b[0m         \u001b[39mraise\u001b[39;00m URLError(err)\n",
      "File \u001b[0;32m/usr/lib/python3.10/http/client.py:1283\u001b[0m, in \u001b[0;36mHTTPConnection.request\u001b[0;34m(self, method, url, body, headers, encode_chunked)\u001b[0m\n\u001b[1;32m   1280\u001b[0m \u001b[39mdef\u001b[39;00m \u001b[39mrequest\u001b[39m(\u001b[39mself\u001b[39m, method, url, body\u001b[39m=\u001b[39m\u001b[39mNone\u001b[39;00m, headers\u001b[39m=\u001b[39m{}, \u001b[39m*\u001b[39m,\n\u001b[1;32m   1281\u001b[0m             encode_chunked\u001b[39m=\u001b[39m\u001b[39mFalse\u001b[39;00m):\n\u001b[1;32m   1282\u001b[0m \u001b[39m    \u001b[39m\u001b[39m\"\"\"Send a complete request to the server.\"\"\"\u001b[39;00m\n\u001b[0;32m-> 1283\u001b[0m     \u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49m_send_request(method, url, body, headers, encode_chunked)\n",
      "File \u001b[0;32m/usr/lib/python3.10/http/client.py:1329\u001b[0m, in \u001b[0;36mHTTPConnection._send_request\u001b[0;34m(self, method, url, body, headers, encode_chunked)\u001b[0m\n\u001b[1;32m   1325\u001b[0m \u001b[39mif\u001b[39;00m \u001b[39misinstance\u001b[39m(body, \u001b[39mstr\u001b[39m):\n\u001b[1;32m   1326\u001b[0m     \u001b[39m# RFC 2616 Section 3.7.1 says that text default has a\u001b[39;00m\n\u001b[1;32m   1327\u001b[0m     \u001b[39m# default charset of iso-8859-1.\u001b[39;00m\n\u001b[1;32m   1328\u001b[0m     body \u001b[39m=\u001b[39m _encode(body, \u001b[39m'\u001b[39m\u001b[39mbody\u001b[39m\u001b[39m'\u001b[39m)\n\u001b[0;32m-> 1329\u001b[0m \u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49mendheaders(body, encode_chunked\u001b[39m=\u001b[39;49mencode_chunked)\n",
      "File \u001b[0;32m/usr/lib/python3.10/http/client.py:1278\u001b[0m, in \u001b[0;36mHTTPConnection.endheaders\u001b[0;34m(self, message_body, encode_chunked)\u001b[0m\n\u001b[1;32m   1276\u001b[0m \u001b[39melse\u001b[39;00m:\n\u001b[1;32m   1277\u001b[0m     \u001b[39mraise\u001b[39;00m CannotSendHeader()\n\u001b[0;32m-> 1278\u001b[0m \u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49m_send_output(message_body, encode_chunked\u001b[39m=\u001b[39;49mencode_chunked)\n",
      "File \u001b[0;32m/usr/lib/python3.10/http/client.py:1038\u001b[0m, in \u001b[0;36mHTTPConnection._send_output\u001b[0;34m(self, message_body, encode_chunked)\u001b[0m\n\u001b[1;32m   1036\u001b[0m msg \u001b[39m=\u001b[39m \u001b[39mb\u001b[39m\u001b[39m\"\u001b[39m\u001b[39m\\r\u001b[39;00m\u001b[39m\\n\u001b[39;00m\u001b[39m\"\u001b[39m\u001b[39m.\u001b[39mjoin(\u001b[39mself\u001b[39m\u001b[39m.\u001b[39m_buffer)\n\u001b[1;32m   1037\u001b[0m \u001b[39mdel\u001b[39;00m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39m_buffer[:]\n\u001b[0;32m-> 1038\u001b[0m \u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49msend(msg)\n\u001b[1;32m   1040\u001b[0m \u001b[39mif\u001b[39;00m message_body \u001b[39mis\u001b[39;00m \u001b[39mnot\u001b[39;00m \u001b[39mNone\u001b[39;00m:\n\u001b[1;32m   1041\u001b[0m \n\u001b[1;32m   1042\u001b[0m     \u001b[39m# create a consistent interface to message_body\u001b[39;00m\n\u001b[1;32m   1043\u001b[0m     \u001b[39mif\u001b[39;00m \u001b[39mhasattr\u001b[39m(message_body, \u001b[39m'\u001b[39m\u001b[39mread\u001b[39m\u001b[39m'\u001b[39m):\n\u001b[1;32m   1044\u001b[0m         \u001b[39m# Let file-like take precedence over byte-like.  This\u001b[39;00m\n\u001b[1;32m   1045\u001b[0m         \u001b[39m# is needed to allow the current position of mmap'ed\u001b[39;00m\n\u001b[1;32m   1046\u001b[0m         \u001b[39m# files to be taken into account.\u001b[39;00m\n",
      "File \u001b[0;32m/usr/lib/python3.10/http/client.py:976\u001b[0m, in \u001b[0;36mHTTPConnection.send\u001b[0;34m(self, data)\u001b[0m\n\u001b[1;32m    974\u001b[0m \u001b[39mif\u001b[39;00m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39msock \u001b[39mis\u001b[39;00m \u001b[39mNone\u001b[39;00m:\n\u001b[1;32m    975\u001b[0m     \u001b[39mif\u001b[39;00m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39mauto_open:\n\u001b[0;32m--> 976\u001b[0m         \u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49mconnect()\n\u001b[1;32m    977\u001b[0m     \u001b[39melse\u001b[39;00m:\n\u001b[1;32m    978\u001b[0m         \u001b[39mraise\u001b[39;00m NotConnected()\n",
      "File \u001b[0;32m/usr/lib/python3.10/http/client.py:1448\u001b[0m, in \u001b[0;36mHTTPSConnection.connect\u001b[0;34m(self)\u001b[0m\n\u001b[1;32m   1445\u001b[0m \u001b[39mdef\u001b[39;00m \u001b[39mconnect\u001b[39m(\u001b[39mself\u001b[39m):\n\u001b[1;32m   1446\u001b[0m     \u001b[39m\"\u001b[39m\u001b[39mConnect to a host on a given (SSL) port.\u001b[39m\u001b[39m\"\u001b[39m\n\u001b[0;32m-> 1448\u001b[0m     \u001b[39msuper\u001b[39;49m()\u001b[39m.\u001b[39;49mconnect()\n\u001b[1;32m   1450\u001b[0m     \u001b[39mif\u001b[39;00m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39m_tunnel_host:\n\u001b[1;32m   1451\u001b[0m         server_hostname \u001b[39m=\u001b[39m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39m_tunnel_host\n",
      "File \u001b[0;32m/usr/lib/python3.10/http/client.py:942\u001b[0m, in \u001b[0;36mHTTPConnection.connect\u001b[0;34m(self)\u001b[0m\n\u001b[1;32m    940\u001b[0m \u001b[39m\u001b[39m\u001b[39m\"\"\"Connect to the host and port specified in __init__.\"\"\"\u001b[39;00m\n\u001b[1;32m    941\u001b[0m sys\u001b[39m.\u001b[39maudit(\u001b[39m\"\u001b[39m\u001b[39mhttp.client.connect\u001b[39m\u001b[39m\"\u001b[39m, \u001b[39mself\u001b[39m, \u001b[39mself\u001b[39m\u001b[39m.\u001b[39mhost, \u001b[39mself\u001b[39m\u001b[39m.\u001b[39mport)\n\u001b[0;32m--> 942\u001b[0m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39msock \u001b[39m=\u001b[39m \u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49m_create_connection(\n\u001b[1;32m    943\u001b[0m     (\u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49mhost,\u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49mport), \u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49mtimeout, \u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49msource_address)\n\u001b[1;32m    944\u001b[0m \u001b[39m# Might fail in OSs that don't implement TCP_NODELAY\u001b[39;00m\n\u001b[1;32m    945\u001b[0m \u001b[39mtry\u001b[39;00m:\n",
      "File \u001b[0;32m/usr/lib/python3.10/socket.py:833\u001b[0m, in \u001b[0;36mcreate_connection\u001b[0;34m(address, timeout, source_address)\u001b[0m\n\u001b[1;32m    831\u001b[0m \u001b[39mif\u001b[39;00m source_address:\n\u001b[1;32m    832\u001b[0m     sock\u001b[39m.\u001b[39mbind(source_address)\n\u001b[0;32m--> 833\u001b[0m sock\u001b[39m.\u001b[39;49mconnect(sa)\n\u001b[1;32m    834\u001b[0m \u001b[39m# Break explicitly a reference cycle\u001b[39;00m\n\u001b[1;32m    835\u001b[0m err \u001b[39m=\u001b[39m \u001b[39mNone\u001b[39;00m\n",
      "\u001b[0;31mKeyboardInterrupt\u001b[0m: "
     ]
    }
   ],
   "source": [
    "# OpenDrive to CommonRoad\n",
    "from crdesigner.map_conversion.osm2cr.converter_modules.osm_operations.downloader import download_around_map\n",
    "from crdesigner.map_conversion.osm2cr import config\n",
    "from crdesigner.map_conversion.map_conversion_interface import opendrive_to_commonroad\n",
    "\n",
    "download_around_map(config.BENCHMARK_ID + 'download.osm', 31.2708, 121.168, 2000)\n",
    "# 使用 API 的方式下载地图失败，已经手工下载到 scenario/commonroad/map.osm"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 整体的 API\n",
    "from crdesigner.map_conversion.map_conversion_interface import osm_to_commonroad\n",
    "\n",
    "# 加载 OSM 文件\n",
    "scenario = osm_to_commonroad(os.path.join(ROOT_DIR, 'scenario/osm/QiCheChengDiTieZhan.osm'))\n",
    "\n",
    "# 保存 cr 格式的场景文件\n",
    "writer = CommonRoadFileWriter(\n",
    "    scenario=scenario,\n",
    "    planning_problem_set=PlanningProblemSet(),\n",
    "    author='zhaoshucheng',\n",
    "    affiliation='Tongji University',\n",
    "    source='CommonRoad Scenario Designer',\n",
    "    tags={Tag.URBAN},\n",
    ")\n",
    "writer.write_to_file(os.path.join(ROOT_DIR, 'scenario/commonroad/QiCheChengDiTieZhan.xml'), OverwriteExistingFile.ALWAYS)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 展示 OSM Conversion APIs\n",
    "\n",
    "import crdesigner.map_conversion.osm2cr.converter_modules.converter as converter\n",
    "import crdesigner.map_conversion.osm2cr.converter_modules.cr_operations.export as ex\n",
    "\n",
    "osm_path = os.path.join(ROOT_DIR, 'scenario/osm/QiCheChengDiTieZhan.osm')\n",
    "xml_path = os.path.join(ROOT_DIR, 'scenario/commonroad/QiCheChengDiTieZhan.xml')\n",
    "pickle_path = os.path.join(ROOT_DIR, 'scenario/commonroad/QiCheChengDiTieZhan.pickle')\n",
    "\n",
    "# 打开一个地图，然后将其转换成一个 scenario\n",
    "scenario = converter.GraphScenario(osm_path)\n",
    "\n",
    "# 绘制并且展示 scenario\n",
    "scenario.plot()\n",
    "\n",
    "# 保存这个 scenario 作为一个 commonroad file\n",
    "scenario.save_as_cr(xml_path)\n",
    "\n",
    "# 保存这个场景作为一个 binary\n",
    "scenario.save_as_cr(pickle_path)\n",
    "\n",
    "# 展示生成的结果\n",
    "# ex.view_xml(xml_path)    # 这里有问题, 所以暂时注释\n",
    "'''\n",
    "File ~/Programs/benchmarker_learn/.venv/lib/python3.10/site-packages/commonroad/visualization/mp_renderer.py:116, in MPRenderer.__init__(self, draw_params, plot_limits, ax, figsize, focus_obstacle)\n",
    "    114 self.traffic_signs = []\n",
    "    115 self.traffic_sign_call_stack = tuple()\n",
    "--> 116 self._traffic_sign_draw_params = self.draw_params.traffic_sign\n",
    "    117 self._traffic_light_draw_params = self.draw_params.traffic_light\n",
    "    118 # labels of dynamic elements\n",
    "'''\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 3 OpenDRIVE to CommonRoad\n",
    "input_path = os.path.join(ROOT_DIR, 'scenario/opendrive/QiCheChengDiTieZhan.xodr')\n",
    "\n",
    "from crdesigner.map_conversion.map_conversion_interface import opendrive_to_commonroad, commonroad_to_sumo\n",
    "from lxml import etree\n",
    "\n",
    "# 将 openDRIVE 文件转换为 scenario\n",
    "scenario = opendrive_to_commonroad(input_path)    # 这里会报错\n",
    "\n",
    "# 储存转换的场景文件作为 CommonRoad 场景\n",
    "writer = CommonRoadFileWriter(\n",
    "    scenario=scenario,\n",
    "    planning_problem_set=PlanningProblemSet(),\n",
    "    author='zhaoshucheng',\n",
    "    affiliation='TongJi University',\n",
    "    source='CommonRoad Scenario Designer',\n",
    "    tags={Tag.URBAN},\n",
    ")\n",
    "writer.write_to_file(os.path.join(ROOT_DIR, 'scenario/commonroad/QiCheChengDiTieZhan_openDRIVE_to_commonraod.xml'), OverwriteExistingFile.ALWAYS)\n",
    "\n",
    "\n",
    "# 展示 OpenDRIVE 转换 APIs\n",
    "from crdesigner.map_conversion.opendrive.opendrive_parser.parser import parse_opendrive\n",
    "from crdesigner.map_conversion.opendrive.opendrive_conversion.network import Network\n",
    "\n",
    "# 使用 OpenDRIVE parser to load file\n",
    "print(input_path)\n",
    "# with open(input_path, 'r') as file_in:\n",
    "opendrive = parse_opendrive(input_path)\n",
    "\n",
    "# 创建路网中间对象\n",
    "road_network = Network()\n",
    "\n",
    "# 转换成 OpenDRIVE 文件\n",
    "# 先解析 opendrive 文件，然后构造 NetWork 对象，加载前面解析 opendrive 创建的对象\n",
    "road_network.load_opendrive(opendrive)\n",
    "\n",
    "scenario = road_network.export_commonroad_scenario()\n",
    "\n",
    "# 储存由 opendrive 转换的文件\n",
    "writer = CommonRoadFileWriter(\n",
    "    scenario=scenario,\n",
    "    planning_problem_set=PlanningProblemSet(),\n",
    "    author='zhaoshucheng',\n",
    "    affiliation='TongJi Nuiversity',\n",
    "    source='Commonroad',\n",
    "    tags={Tag.URBAN},\n",
    ")\n",
    "writer.write_to_file(os.path.join(ROOT_DIR, 'scenario/commonroad/QiCheChengDiTieZhan_openDRIVE_to_commonraod_2.xml'), OverwriteExistingFile.ALWAYS)\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Lanelet/Lanelet2 转换为 CommonRoad 以及 CommonRoad 转换为 Lanelet\n",
    "\n",
    "# 这一块需要了解一下 Lanelet2 的格式再说\n",
    "\n",
    "# from crdesigner.map_conversion.map_conversion_interface import commonroad_to_lanelet\n",
    "# from crdesigner.map_conversion.lanelet2.cr2lanelet import CR2LaneletConverter\n",
    "\n",
    "# cr_scenario_path = os.path.join(ROOT_DIR, 'scenario/commonroad/QiCheChengDiTieZhan_openDRIVE_to_commonraod.xml')\n",
    "# # lanelet_out_path\n",
    "# # CR2LaneletConverter\n",
    "# try:\n",
    "#     commonroad_reader = "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# SUMO 和 CommonRoad 之间的交互\n",
    "\n",
    "input_file = os.path.join(ROOT_DIR, 'scenario/sumo/QiCheChengDiTieZhan.net.xml')\n",
    "\n",
    "from crdesigner.map_conversion.map_conversion_interface import sumo_to_commonroad\n",
    "\n",
    "# 加载 SUMO 网络文件，解析它，然后转换为 common Road 地图\n",
    "\n",
    "scenario = sumo_to_commonroad(input_file)\n",
    "\n",
    "# 储存 转换后的 CommonRoad 文件\n",
    "writer = CommonRoadFileWriter(\n",
    "    scenario=scenario,\n",
    "    planning_problem_set=PlanningProblemSet(),\n",
    "    author='zhaoshucheng',\n",
    "    affiliation='TongJi',\n",
    "    source='CommonRoad',\n",
    "    tags={Tag.URBAN},\n",
    ")\n",
    "writer.write_to_file(os.path.join(ROOT_DIR, 'scenario/commonroad/QiCheChengDiTieZhan_sumo_to_cr.xml'), OverwriteExistingFile.ALWAYS)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# SUMO 带交通仿真和视频创建的 转换 API\n",
    "from commonroad.visualization.mp_renderer import MPRenderer\n",
    "import numpy as np\n",
    "from crdesigner.ui.gui.mwindow.animated_viewer_wrapper.gui_sumo_simulation import SUMO_AVAILABLE\n",
    "\n",
    "if SUMO_AVAILABLE:\n",
    "    from crdesigner.map_conversion.sumo_map.config import SumoConfig\n",
    "    from crdesigner.map_conversion.sumo_map.cr2sumo.converter import CR2SumoMapConverter\n",
    "    from sumocr.interface.sumo_simulation import SumoSimulation\n",
    "    from sumocr.visualization.video import create_video\n",
    "\n",
    "# 设置 commonroad 文件\n",
    "input_file = os.path.join(ROOT_DIR, 'scenario/commonroad/QiCheChengDiTieZhan_sumo_to_cr.xml')\n",
    "\n",
    "# 构建 scenario, planning_problem\n",
    "scenario, planning_problem = CommonRoadFileReader(input_file).open()\n",
    "\n",
    "# 转换场景到中心 找到当前场景的中心点\n",
    "centroid = np.mean(np.concatenate(\n",
    "    [la.center_vertices for la in scenario.lanelet_network.lanelets]),axis=0)\n",
    "scenario.translate_rotate(-centroid, 0)\n",
    "planning_problem.translate_rotate(-centroid, 0)\n",
    "\n",
    "config = SumoConfig.from_scenario_name('QiCheChengDiTieZhan')\n",
    "\n",
    "# 将 cr 转换为 sumo net\n",
    "wapper = CR2SumoMapConverter(scenario, config)\n",
    "# wapper.convert_to_net_file(os.path.join(ROOT_DIR, 'scenario/sumo/QiCheChengDiTieZhan_cr_to_sumo.net.xml')) # TODO 没有这个函数，暂时跳过\n",
    "\n",
    "# 自动生成交通信号灯\n",
    "tls_lanelet_id = 43513\n",
    "traffic_light_system_generated = wapper.auto_generate_traffic_light_system(tls_lanelet_id)\n",
    "print(f\"在 {tls_lanelet_id} 生成了交通信号灯 {traffic_light_system_generated}\")\n",
    "\n",
    "\n",
    "# 在场景生成完交通灯后，绘制场景\n",
    "rnd = MPRenderer()\n",
    "wapper.lanelet_network.draw(rnd)\n",
    "rnd.render(show=True)\n",
    "\n",
    "# 写入修改后的信号灯\n",
    "pass\n",
    "\n",
    "# 执行仿真\n",
    "simulation = SumoSimulation()\n",
    "simulation.initialize(config, wapper)\n",
    "\n",
    "for t in range(config.simulation_steps):\n",
    "    simulation.simulate_step()\n",
    "\n",
    "simulation.stop()\n",
    "\n",
    "\n"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": ".venv",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
